/** Control functions */
//##############################################################################




/** Global variables used mainly to read pid parameters and to write position
 * and orientation status
*/
#include "globals.h"
//------------------------------------------------------------------------------




/** Used for scaling the itg and adxl values */
#include "calibration.h"
//------------------------------------------------------------------------------




/** Sine and cosine, arcsine, arctan2, sqrt functions */
#include <math.h>
//------------------------------------------------------------------------------




/** sprintf */
#include <stdio.h>
//------------------------------------------------------------------------------




/** For controlling the timer from within the handler and to start the timer in
 * the first place.
*/
#include "LR_timer.h"
//------------------------------------------------------------------------------




/** Used to send the quaternion via USART */
#include "LR_usart.h"
//------------------------------------------------------------------------------




/** Used to send commands to the engine controls */
#include "LR_twimaster.h"
//------------------------------------------------------------------------------




/** ITG and ADXL abstraction layer to get raw data */
#include "itg.h"
#include "adxl.h"
//##############################################################################




/** The slot the control handler is running on */
volatile unsigned char slot;
//------------------------------------------------------------------------------




/** Sample time in seconds */
volatile double t;
//##############################################################################






/** This is the control handler.
 *
 * This function gets called with a fixed period by a timer interrupt. Its job
 * is to read the current position and orientation and to control these by
 * applying different speeds to the 4 engines.
*/
__attribute__((__interrupt__))
static void control_handler( void )
{
  /*LR_usart_write(".");
  LR_timer_sti( slot );
  return;*/



  // These two short arrays are used to save the raw data comming from the
  // sensors
  short raw_itg [3];
  short raw_adxl [3];
  // The delta angle saves the computed angle difference
  double delta_angle [3];
  //............................................................................
  // Read raw sensor values via TWI interface
  itg_read( &(raw_itg[0]), &(raw_itg[1]), &(raw_itg[2]) );
  adxl_read( &(raw_adxl[0]), &(raw_adxl[1]), &(raw_adxl[2]) );
  // Scale them to reflect correct physical values
  // For scaling them, an offset (from calibration) and a scale factor (from
  // datasheet) is required.
  itg[0] = (raw_itg[0] - calib_itg[0]) * scale_itg[0];
  itg[1] = (raw_itg[1] - calib_itg[1]) * scale_itg[1];
  itg[2] = (raw_itg[2] - calib_itg[2]) * scale_itg[2];
  adxl[0] = (raw_adxl[0] - calib_adxl[0]) * scale_adxl[0];
  adxl[1] = (raw_adxl[1] - calib_adxl[1]) * scale_adxl[1];
  adxl[2] = (raw_adxl[2] - calib_adxl[2]) * scale_adxl[2];
  //............................................................................
  // Compute delta angles.
  // This is a stepwise integration over the sampling time (in seconds)
  delta_angle[0] = itg[0] * t;
  delta_angle[1] = itg[1] * t;
  delta_angle[2] = itg[2] * t;
  //............................................................................
  // 1D rotation angles
  // 1D rotation angles (they are wrong) are computed by simply adding the
  // delta angles.
  rot1D[0] += delta_angle[0];
  rot1D[1] += delta_angle[1];
  rot1D[2] += delta_angle[2];
  //............................................................................
  // These two arrays are used to save the sine and cosine of the delta angles
  // This is done because they are used quite often further down the computation
  // so saving them instead of recalculating saves precious time.
  double _c [3];
  double _s [3];
  // This array of four represents the 4 dimensions of a quaternion. This
  // quaternion will hold the information with corresponds to the rotations
  // around the given delta_angles.
  double _rq [4];
  // This is used to save the length of the quaternion, used to norm it.
  double _rq_l;
  // Compute the sine and cosine terms.
  sincos( delta_angle[0] / 2, &(_s[0]), &(_c[0]) );
  sincos( delta_angle[1] / 2, &(_s[1]), &(_c[1]) );
  sincos( delta_angle[2] / 2, &(_s[2]), &(_c[2]) );
  // Compute the rotation quaternion which reflects the difference in
  // orientation since the last call. The formula is taken from the 
  // LR presentation.
  _rq[0] = _c[2] * _c[1] * _c[0] + _s[2] * _s[1] * _s[0];
  _rq[1] = _c[2] * _c[1] * _s[0] - _s[2] * _s[1] * _c[0];
  _rq[2] = _c[2] * _s[1] * _c[0] + _s[2] * _c[1] * _s[0];
  _rq[3] = _s[2] * _c[1] * _c[0] - _c[2] * _s[1] * _s[0];
  // Norm the resulting rotation quaternion. Quaternions should always be normed
  // when they are used to represent rotations.
  _rq_l = sqrt( _rq[0]*_rq[0] + _rq[1]*_rq[1] + _rq[2]*_rq[2] + _rq[3]*_rq[3] );
  _rq[0] /= _rq_l;
  _rq[1] /= _rq_l;
  _rq[2] /= _rq_l;
  // Multiply with current orientation quaternion. This way, the rotation since
  // the last call gets added to a "overall rotation" describing how to get
  // from the original orientation to the current orientation.
  // This information is saved in "quaternion".
  // q is an array storing the new overall rotation until it is normed.
  double q [4];
  // Those formulas are taken from the LR presentation.
  q[0] = _rq[0] * quaternion[0] - 
    _rq[1] * quaternion[1] - 
    _rq[2] * quaternion[2] - 
    _rq[3] * quaternion[3];
  q[1] = _rq[0] * quaternion[1] +
    _rq[1] * quaternion[0] -
    _rq[2] * quaternion[3] +
    _rq[3] * quaternion[2];
  q[2] = _rq[0] * quaternion[2] +
    _rq[1] * quaternion[3] +
    _rq[2] * quaternion[0] -
    _rq[3] * quaternion[1];
  q[3] = _rq[0] * quaternion[3] -
    _rq[1] * quaternion[2] +
    _rq[2] * quaternion[1] +
    _rq[3] * quaternion[0];
  // Norm the result to get the new overall rotation in form of a quaternion
  double q_l;
  q_l = sqrt( q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3] );
  // The normed quaternion is writen back into "quaternion".
  quaternion[0] = q[0] / q_l;
  quaternion[1] = q[1] / q_l;
  quaternion[2] = q[2] / q_l;
  quaternion[3] = q[3] / q_l;
  // If requested by the user, send the current overall quaternion via USART in
  // a special form described in the LR presentation.
  if (ctl_quaternion)
  {
    // Use a iterator to send only every 50th call.
    static unsigned char iterator = 0;
    if (iterator < 50)
    {
      iterator++;
    }
    else
    {
      iterator = 0;
      char string[30];
      sprintf( string, "$,%f,%f,%f,%f,#",
          quaternion[0], quaternion[1], quaternion[2], quaternion[3] );
      LR_usart_write( string );
    }
  }
  //............................................................................
  // Get current orientation from quaternion and rotate acceleration vector
  // by using quaternion multiplication.
  // To get the current orientation (meaning 3 angles) from the overall
  // quaternion, the formula from the LR presentation is applied.
  // To rotate the acceleration vector (read from the ADXL sensor at the start
  // of this function) the vector is "sandwiched" by the overall quaternion:
  // v_global = q * v_lokal * q'
  // The accel_glob array will be used to store the rotate acceleration vector.
  double accel_glob [3];
  // This array is used to save the square values of the four elements of the
  // orientation quaternion.
  double _q_sq [4];
  // This is a temporary store for square(q0) - square(q1). This is done because
  // the value is needed more than once in the following computation.
  double _q_sq_0_minus_1;
  // Temporary store for square(q2) + square(q3)
  double _q_sq_2_plus_3;
  // Temporary store for square(q2) - square(q3) which is the same as
  // -1 * [ - square(q2) + square(q3) ]
  double _q_sq_2_minus_3;
  // The following variables store more temporary information which is needed
  // several times in the following computation.
  // They are all of the form _qA_qB and save the value of qA * qB
  double _q0_q1;
  double _q0_q2;
  double _q0_q3;
  double _q1_q2;
  double _q1_q3;
  double _q2_q3;
  // Compute the square values of each quaternion element for later use.
  // There is an alternate (possibly faster) method to calculate the last of
  // them: _q_sq[3] = 1 -_q_sq[0] - _q_sq[1] - _q_sq[2];
  // This could be used together with _q_sq_2_plus_3 (and e.g. _q_sq[0])
  _q_sq[0] = quaternion[0] * quaternion[0];
  _q_sq[1] = quaternion[1] * quaternion[1];
  _q_sq[2] = quaternion[2] * quaternion[2];
  _q_sq[3] = quaternion[3] * quaternion[3];
  // Compute summs and differences of some squares for later use.
  _q_sq_0_minus_1 = _q_sq[0] - _q_sq[1];
  _q_sq_2_plus_3 = _q_sq[2] + _q_sq[3];
  _q_sq_2_minus_3 = _q_sq[2] - _q_sq[3];
  // Compute products used later.
  _q0_q1 = quaternion[0] * quaternion[1];
  _q0_q2 = quaternion[0] * quaternion[2];
  _q0_q3 = quaternion[0] * quaternion[3];
  _q1_q2 = quaternion[1] * quaternion[2];
  _q1_q3 = quaternion[1] * quaternion[3];
  _q2_q3 = quaternion[2] * quaternion[3];
  // Extract the current orientation from the quaternion. This forumla is given
  // on: en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
  // The computation uses the precalculated values from above.
  // TODO aviod possible singularities as explained there:
  // http://www.euclideanspace.com/maths/geometry/rotations/
  //                            conversions/quaternionToEuler/indexLocal.htm
  orientation[0] = atan2( 2 * (_q0_q1 + _q2_q3), 1 - 2 *(_q_sq[1] + _q_sq[2]) );
  orientation[1] = asin( 2 * (_q0_q2 - _q1_q3) );
  orientation[2] = atan2( 2 * (_q0_q3 + _q1_q2), 1 - 2 * (_q_sq_2_plus_3) );
  // Rotate the acceleration vector to the global refrence system. This is done
  // using the original form:
  // v_global = v_local + 2 (q_vec x ((q_vec x v_local) + (q_0 * v_local)))
  // from:
  // en.wikipedia.org/wiki/Quaternions_and_spatial_rotation
  // http://code.google.com/p/kri/wiki/Quaternions
  // And changing some terms to save some multiplications and/or additions.
  // Requires 22 multiplications (including above precalculations) and
  // 19 summs/differences. TODO: this could be further optimized.
  accel_glob[0] = adxl[0] * (_q_sq[0] + _q_sq[1] - _q_sq_2_plus_3) + 
    2 * (adxl[2] * (_q0_q2+_q1_q3) + adxl[1] * (_q1_q2-_q0_q3));
  accel_glob[1] = adxl[1] * (_q_sq_0_minus_1 + _q_sq_2_minus_3) + 
    2 * (adxl[0] * (_q1_q2+_q0_q3) + adxl[2] * (_q2_q3-_q0_q1));
  accel_glob[2] = adxl[2] * (_q_sq_0_minus_1 - _q_sq_2_minus_3) +
    2 * (adxl[0] * (_q1_q3-_q0_q2) + adxl[1] * (_q2_q3+_q0_q1));
  //............................................................................
  // Calculate new position based on overall speed and acceleration
  // This array contains the value of accel_glob * t, which is the delta speed
  // vector in the global reference system.
  double _d_speed [3];
  _d_speed[0] = accel_glob[0] * t;
  _d_speed[1] = accel_glob[1] * t;
  _d_speed[2] = accel_glob[2] * t;
  // The new position is calculated by p(n+1) = p(n) + v(n)*t + 0.5*a*t*t
  position[0] += (speed[0] + 0.5 * _d_speed[0]) * t;
  position[1] += (speed[1] + 0.5 * _d_speed[1]) * t;
  position[2] += (speed[2] + 0.5 * _d_speed[2]) * t;
  // The new overall speed vector is computed by adding the delta speed vector
  // v(n+1) = v(n)*t
  speed[0] += _d_speed[0];
  speed[1] += _d_speed[1];
  speed[2] += _d_speed[2];
  //............................................................................
  //............................................................................
  // PID controllers for all x,y,z position, attitude (roll+pitch) and yaw.
  // Each of them (position, attitude, yaw) can be enabled seperately. Only if
  // position control is enabled, attitude control will be enabled, too.
  // The reason is, that the position gets controlled via altering the
  // reference angles of the attitude controller.
  // This variable hold the result (out) of the pid controller for the z
  // position, aka height.
  double pid_height_out;
  // This array contains the reference angles for the two attitude pid
  // controllers.
  double pid_attitude_in [2];
  // This array will be set to the result (out) of the two pid controllers for
  // the attitude.
  double pid_attitude_out [2] = {0, 0};
  // The yaw controller uses the global reference value by default, so there is
  // no need for a special "in" value as it takes the global value directly.
  // But the result of the yaw controller is saved into a variable, which is the
  // following:
  double pid_yaw_out = 0;
  //............................................................................
  // If the user requests the system to handle the position of the quadrocopter,
  // then use the pid controller to maintain the system at position 0,0,0
  // The system may move then when the maincode adds appropriate values to the
  // current position, such that the controller thinks it is off position and
  // corrects its position to the new one.
  if (ctl_position)
  {
    // Use position as error.
    // To change the position, one has then to add the difference onto the
    // current position
    // Position uses attitude control to move (reference angles).
    // The needed values for the two reference angles are saved into the
    // following array:
    double _pid_angle_out [2];
    // Apply P gain to error (which is equal to the position but TODO may need
    // a minus) and save to result variables.
    _pid_angle_out[0] = pid_position[0] * position[0];
    _pid_angle_out[1] = pid_position[0] * position[1];
    pid_height_out = pid_position[0] * position[2];
    // Integrate the next step, multiplying the position/error with t, the
    // sampling time in seconds.
    // TODO check for over/underflows.
    pid_position_integral[0] += position[0] * t;
    pid_position_integral[1] += position[1] * t;
    pid_position_integral[2] += position[2] * t;
    // Applying the I gain to the integral values and adding the result to the
    // out variables of the three controllers.
    _pid_angle_out[0] += pid_position[1] * pid_position_integral[0];
    _pid_angle_out[1] += pid_position[1] * pid_position_integral[1];
    pid_height_out += pid_position[1] * pid_position_integral[2];
    // Using D gain and the error from the last call to build approximately the
    // derivative of the error "function" and add this to the controllers
    // results.
    _pid_angle_out[0] += pid_position[2] *
      (position[0] - pid_position_last[0]) / t;
    _pid_angle_out[1] += pid_position[2] *
      (position[1] - pid_position_last[1]) / t;
    pid_height_out += pid_position[2] *
      (position[2] - pid_position_last[2]) / t;
    // Save error for the next call.
    pid_position_last[0] = position[0];
    pid_position_last[1] = position[1];
    pid_position_last[2] = position[2];
    // Compute sine and cosine of the yaw (z) angle to distribute the result of
    // the position pid controllers in the z plane to the two in values of the
    // attitude controllers (which are independent from the rotation around the
    // z axis).
    double _s;
    double _c;
    sincos( orientation[2], &_s, &_c );
    // Apply the position, according to a circle with radius 1.
    // TODO The sine cosines may need to be switched.
    pid_attitude_in[0] = _s * _pid_angle_out[0] + _c * _pid_angle_out[1];
    pid_attitude_in[1] = _c * _pid_angle_out[0] + _s * _pid_angle_out[1];
  }
  else
  {
    // If position control is not enabled, use the global variables as reference
    // values for the attitude controller. This way, the user can specify his
    // own angles (but of course has to fix the quadrocopter)
    pid_attitude_in[0] = attitude_reference[0];
    pid_attitude_in[1] = attitude_reference[1];
    // A fix value for the output of the height controller, this may be used
    // later TODO as idle offset to the engines.
    pid_height_out = 0; // TODO
  }
  //............................................................................
  // When the user wants to control the attitude (or position) of the
  // quadrocopter, then we need to compute the PID controller output values
  // to steer the engines correctly.
  if (ctl_attitude || ctl_position)
  {
    // This variable shall be used to save the error of the attitude.
    double _error [2];
    // The error is computed from current orientation and reference attitude.
    // This is either a value set by the user or the result of the position
    // controller.
    _error[0] = orientation[0] - pid_attitude_in[0];
    _error[1] = orientation[1] - pid_attitude_in[1];
    // The controllers result is set to the error with applied P gain.
    pid_attitude_out[0] = pid_attitude[0] * _error[0];
    pid_attitude_out[1] = pid_attitude[0] * _error[1];
    // The integral is built. TODO check for over/underflows.
    pid_attitude_integral[0] += _error[0] * t;
    pid_attitude_integral[1] += _error[1] * t;
    // Using I gain to apply integrated error to the controllers results.
    pid_attitude_out[0] += pid_attitude[1] * pid_attitude_integral[0];
    pid_attitude_out[1] += pid_attitude[1] * pid_attitude_integral[1];
    // Using the D gain, the error from the last call and t the sampling time to
    // derive the error "function" and apply it to the controllers results.
    pid_attitude_out[0] += pid_attitude[2] *
      (_error[0] - pid_attitude_last[0]) / t;
    pid_attitude_out[1] += pid_attitude[2] *
      (_error[1] - pid_attitude_last[1]) / t;
    // Save the errors for the next call.
    pid_attitude_last[0] = _error[0];
    pid_attitude_last[1] = _error[1];
  }
  //............................................................................
  // If the user request the control of the yaw parameter of the quadrocopter,
  // then this PID controller is used to maintain a stable state at the
  // reference yaw angle.
  // Neither the attitude or the position controllers need assistance from this
  // controller, but activating it should result in a much more stable system!
  if (ctl_yaw)
  {
    // This variable is used to store the error which is used to calculate
    // the controllers result.
    double _error;
    // The error is calculated from the current yaw angle, saved in
    // orientation[2] (2 := z axis) and the reference angle which must be set
    // by the user.
    _error = orientation[2] - yaw_reference;
    // Apply the P gain to the error and save as first part of the controllers
    // result.
    pid_yaw_out = pid_yaw[0] * _error;
    // Integrate (stepwise) the error using the sample time t.
    // TODO check for over/underflow.
    pid_yaw_integral += _error * t;
    // Apply I gain to the integrated error and add to controllers result.
    pid_yaw_out += pid_yaw[1] * pid_yaw_integral;
    // Apply D gain to the derived error (using last calls error value).
    pid_yaw_out += pid_yaw[2] * (_error - pid_yaw_last) / t;
    // Save the error for the next call.
    pid_yaw_last = _error;
  }
  //............................................................................
  // The results of all activeted controllers must be applied to the engines
  // speed set values. But only IF the engine control has been activated. This
  // is part of the system is essential to the overall safety!
  // Save the engine control values into this 4 unsigned char values (0 - 255).
  unsigned char engine [4];
  // Apply only if engine control is explicitly set
  if (ctl_engine)
  {
    // Array to store the engine control values until they are scaled and
    // cropped to fit in the range from 0 to 255.
    double _engine [4];
    // Apply height controller value to all engines.
    // Apply first attitude controller to one engine and the negative value
    // to the opposite engine. Use the second controllers value analog at the
    // two remainging engines.
    // ^
    // |-----x-----|
    //             v
    // Apply yaw controler to two opposite engines and subract for the
    // other two engines. This will result in a momentum around the z axis,
    // which is what is desired.
    //       -
    //       .
    //       .
    // +.....o.....+
    //       .
    //       .
    //       -
    _engine[0] = pid_height_out + pid_attitude_out[0] + pid_yaw_out;
    _engine[1] = pid_height_out + pid_attitude_out[1] - pid_yaw_out;
    _engine[2] = pid_height_out - pid_attitude_out[0] + pid_yaw_out;
    _engine[3] = pid_height_out - pid_attitude_out[1] - pid_yaw_out;
    // TODO eventually scale values to make use of finer pid values
    // Cropp the values such that they are not less than 0 and not more than 255
    if (_engine[0] < 0)
    {
      _engine[0] = 0;
    }
    else if (_engine[0] > 255)
    {
      _engine[0] = 255;
    }
    if (_engine[1] < 0)
    {
      _engine[1] = 0;
    }
    else if (_engine[1] > 255)
    {
      _engine[1] = 255;
    }
    if (_engine[2] < 0)
    {
      _engine[2] = 0;
    }
    else if (_engine[2] > 255)
    {
      _engine[2] = 255;
    }
    if (_engine[3] < 0)
    {
      _engine[3] = 0;
    }
    else if (_engine[3] > 255)
    {
      _engine[3] = 255;
    }
  }
  // If engine control is deactivated, use 0 to stop all engines.
  // TODO do NOT remove this, otherwise the engine control values could have
  // undefined values, causing the engines to go wild!
  else
  {
    engine[0] = 0;
    engine[1] = 0;
    engine[2] = 0;
    engine[3] = 0;
  }
  //............................................................................
  // TODO Send command to engines, correct?
  // TODO Engines in 11 Bit mode for finer adjustments?
  //LR_twimaster_send( 0x29, 0 /* ? */, 1, (void *)&(engine[0]), 1 );
  //LR_twimaster_send( 0x2A, 0 /* ? */, 1, (void *)&(engine[1]), 1 );
  //LR_twimaster_send( 0x2B, 0 /* ? */, 1, (void *)&(engine[2]), 1 );
  //LR_twimaster_send( 0x2C, 0 /* ? */, 1, (void *)&(engine[3]), 1 );
  //............................................................................
  // Allow next interrupt to happen.
  char str[50];
  sprintf(str,"%d,%d,%d,%d",engine[0],engine[1],engine[2],engine[3]);
  LR_timer_sti( slot );
}
//------------------------------------------------------------------------------




/** Start the control handler */
void control_handler_start( unsigned char s, unsigned int period )
{
  slot = s;
  t = period / 1000.0;
  LR_timer_assign( slot, control_handler, period );
  LR_timer_start( slot );
}
//##############################################################################




